﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Contacts/b2Contact.h
// Revision   : r141
// Change-Date: 2011-02-03
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp
// Revision   : r112
// Change-Date: 2010-06-17
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public delegate Contact ContactCreateFcn(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB);
    public delegate void ContactDestroyFcn(ref Contact contact);

    public class ContactRegister
    {
        public ContactCreateFcn createFcn;
        public ContactDestroyFcn destroyFcn;
        public bool primary;
    };

    /// <summary>
    /// A contact edge is used to connect bodies and contacts together
    /// in a contact graph where each body is a node and each contact
    /// is an edge. A contact edge belongs to a doubly linked list
    /// maintained in each attached body. Each contact has two contact
    /// nodes, one for each attached body.
    /// </summary>
    public class ContactEdge
    {
        public Body other;			// provides quick access to the other body attached.
        public Contact contact;		// the contact
        public ContactEdge prev;	    // the previous contact edge in the body's contact list
        public ContactEdge next;	    // the next contact edge in the body's contact list
    };

    /// <summary>
    /// The class manages contact between two shapes. A contact exists for each overlapping
    /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
    /// that has no contact points.
    /// </summary>
    public abstract class Contact
    {
        public static ContactRegister[][] s_registers = new ContactRegister[(int)ShapeType.SHAPE_TYPE_COUNT][/*(int)ShapeType.ShapeTypeCount*/]; 

        /// Get the contact manifold.
        public Manifold GetManifold()
        {
            return m_manifold;
        }

        public void SetManifold(Manifold manifold)
        {
            m_manifold = manifold;
        }

        public void GetManifold(out Manifold manifold)
        {
            manifold = m_manifold;
        }

	    /// Get the world manifold.
        public void GetWorldManifold(out WorldManifold worldManifold)
        {
            worldManifold = new WorldManifold();

            Body bodyA = m_fixtureA.GetBody();
            Body bodyB = m_fixtureB.GetBody();
            Shape shapeA = m_fixtureA.GetShape();
            Shape shapeB = m_fixtureB.GetShape();

            worldManifold.Initialize(m_manifold, bodyA.Transform, shapeA.m_radius, bodyB.Transform, shapeB.m_radius);
        }

        /// <summary>
        /// Is this contact touching.
        /// </summary>
        /// <returns></returns>
        public bool IsTouching()
        {
            return (m_flags & ContactFlags.e_touchingFlag) == ContactFlags.e_touchingFlag;
        }

        /// <summary>
        /// Enable/disable this contact. This can be used inside the pre-solve 
        /// contact listener. The contact is only disabled for the current 
        /// time step (or sub-step in continuous collisions). 
        /// </summary>
        public void SetEnabled(bool flag)
        {
            if (flag)
                m_flags |= ContactFlags.e_enabledFlag;
            else
                m_flags &= ~ContactFlags.e_enabledFlag;
        }

        /// <summary>
        /// Has this contact been disabled? 
        /// </summary>
        public bool IsEnabled()
        {
            return (m_flags & ContactFlags.e_enabledFlag) == ContactFlags.e_enabledFlag;
        }
 
	    /// Get the next contact in the world's contact list.
        public Contact GetNext()
        {
            return m_next;
        }

	    /// Get fixture A in this contact.
        public Fixture GetFixtureA()
        {
            return m_fixtureA;
        }

        /// <summary>
        /// Get the child primitive index for fixture A.
        /// </summary>
        public int GetChildIndexA()
        {
            return m_indexA;
        }

	    /// Get fixture B in this contact.
        public Fixture GetFixtureB()
        {
            return m_fixtureB;
        }

        /// <summary>
        /// Get the child primitive index for fixture B.
        /// </summary>
        public int GetChildIndexB()
        {
            return m_indexB;
        }

        /// <summary>
        /// Override the default friction mixture. You can call this in ContactListener.PreSolve
        /// </summary>
        public virtual float Friction
        {
            get;
            set;
        }

        /// <summary>
        /// Reset the friction mixture to the default value.
        /// </summary>
        public virtual void ResetFriction()
        {

        }

        /// <summary>
        /// Override the default restitution mixture. You can call this in ContactListener.PreSolve
        /// </summary>
        public virtual float Restitution
        {
            get;
            set;
        }

        /// <summary>
        /// Reset the Restitution to the default value.
        /// </summary>
        public virtual void ResetRestitution()
        {

        }

        /// <summary>
        /// Flag this contact for filtering. Filtering will occur the next time step.
        /// </summary>
        internal void FlagForFiltering()
        {
            m_flags |= ContactFlags.e_filterFlag;
        }

        public abstract void Evaluate(out Manifold manifold, ref Transform xfA, ref Transform xfB);

	    //--------------- Internals Below -------------------
        [Flags]
        public enum ContactFlags
	    {
            // Used when crawling contact graph when forming islands. 
            e_islandFlag = 0x0001,
            // Set when the shapes are touching. 
            e_touchingFlag = 0x0002,
            // This contact can be disabled (by user) 
            e_enabledFlag = 0x0004,
            // This contact needs filtering because a fixture filter was changed. 
            e_filterFlag = 0x0008,
            // This bullet contact had a TOI event
            e_bulletHitFlag = 0x0010,
            // This contact has a valid TOI in m_toi
            e_toiFlag       = 0x0020,
        };

        protected static void AddType(ContactCreateFcn createFcn, ContactDestroyFcn destroyFcn, ShapeType type1, ShapeType type2)
        {
            System.Diagnostics.Debug.Assert(ShapeType.UNKNOWN < type1 && type1 < ShapeType.SHAPE_TYPE_COUNT);
            System.Diagnostics.Debug.Assert(ShapeType.UNKNOWN < type2 && type2 < ShapeType.SHAPE_TYPE_COUNT); 
            
            if (s_registers[(int)type1] == null) s_registers[(int)type1] = new ContactRegister[(int)ShapeType.SHAPE_TYPE_COUNT];
            
            if (s_registers[(int)type1][(int)type2] == null) s_registers[(int)type1][(int)type2] = new ContactRegister();
            s_registers[(int)type1][(int)type2].createFcn = createFcn; 
            s_registers[(int)type1][(int)type2].destroyFcn = destroyFcn; 
            s_registers[(int)type1][(int)type2].primary = true; 
            
            if (type1 != type2) 
            {
                if (s_registers[(int)type2][(int)type1] == null) s_registers[(int)type2][(int)type1] = new ContactRegister();
                s_registers[(int)type2][(int)type1].createFcn = createFcn; 
                s_registers[(int)type2][(int)type1].destroyFcn = destroyFcn; 
                s_registers[(int)type2][(int)type1].primary = false; 
            }
        }

        protected static void InitializeRegisters()
        {
            AddType(CircleContact.Create, CircleContact.Destroy, ShapeType.CIRCLE, ShapeType.CIRCLE);
            AddType(PolyAndCircleContact.Create, PolyAndCircleContact.Destroy, ShapeType.POLYGON, ShapeType.CIRCLE);
            AddType(PolygonContact.Create, PolygonContact.Destroy, ShapeType.POLYGON, ShapeType.POLYGON);
            AddType(EdgeAndCircleContact.Create, EdgeAndCircleContact.Destroy, ShapeType.EDGE, ShapeType.CIRCLE);
            AddType(EdgeAndPolygonContact.Create, EdgeAndPolygonContact.Destroy, ShapeType.EDGE, ShapeType.POLYGON);
            AddType(LoopAndCircleContact.Create, LoopAndCircleContact.Destroy, ShapeType.LOOP, ShapeType.CIRCLE);
            AddType(LoopAndPolygonContact.Create, LoopAndPolygonContact.Destroy, ShapeType.LOOP, ShapeType.POLYGON);

            AddType(FluidContact.Create, FluidContact.Destroy, ShapeType.FLUID, ShapeType.FLUID);
            AddType(PolyAndFluidContact.Create, PolyAndFluidContact.Destroy, ShapeType.POLYGON, ShapeType.FLUID);
        }

        internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
        {
            if (s_initialized == false)
            {
                InitializeRegisters();
                s_initialized = true;
            }

            ShapeType type1 = fixtureA.GetType();
            ShapeType type2 = fixtureB.GetType();

            System.Diagnostics.Debug.Assert(ShapeType.UNKNOWN < type1 && type1 < ShapeType.SHAPE_TYPE_COUNT);
            System.Diagnostics.Debug.Assert(ShapeType.UNKNOWN < type2 && type2 < ShapeType.SHAPE_TYPE_COUNT);

            ContactCreateFcn createFcn = null;
            if (s_registers[(int)type1] != null)
            {
                if (s_registers[(int)type1][(int)type2] != null)
                {
                    createFcn = s_registers[(int)type1][(int)type2].createFcn;
                }
            }
            if (createFcn != null)
            {
                if (s_registers[(int)type1][(int)type2].primary)
                {
                    return createFcn(fixtureA, indexA, fixtureB, indexB);
                }
                else
                {
                    return createFcn(fixtureB, indexB, fixtureA, indexA);
                }
            }
            else
            {
                return null;
            }
        }

        internal static void Destroy(ref Contact contact, ShapeType typeA, ShapeType typeB)
        {
            throw new NotImplementedException();
        }

        internal static void Destroy(ref Contact contact)
        {
            System.Diagnostics.Debug.Assert(s_initialized == true);

            if (contact.m_manifold.pointCount > 0)
            {
                contact.GetFixtureA().GetBody().Awake = true;
                contact.GetFixtureB().GetBody().Awake = true;
            }

            ShapeType typeA = contact.GetFixtureA().GetType();
            ShapeType typeB = contact.GetFixtureB().GetType();

            System.Diagnostics.Debug.Assert(ShapeType.UNKNOWN < typeA && typeB < ShapeType.SHAPE_TYPE_COUNT);
            System.Diagnostics.Debug.Assert(ShapeType.UNKNOWN < typeA && typeB < ShapeType.SHAPE_TYPE_COUNT);

            ContactDestroyFcn destroyFcn = s_registers[(int)typeA][(int)typeB].destroyFcn;
            ManifoldPool.Instance.Release(contact.m_manifold);
            destroyFcn(ref contact);
        }

	    protected Contact()
        {
            m_fixtureA = null;
            m_fixtureB = null;
        }

        protected Contact(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
        {
            Initialize(fixtureA, indexA, fixtureB, indexB);
        }

        protected internal void Initialize(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
        {
            m_flags = ContactFlags.e_enabledFlag;

            m_fixtureA = fixtureA;
            m_fixtureB = fixtureB;

            m_indexA = indexA;
            m_indexB = indexB;

            //m_manifold = new Manifold();
            m_manifold = ManifoldPool.Instance.Allocate();
            m_manifold.pointCount = 0;

            m_prev = null;
            m_next = null;

            //m_nodeA = new ContactEdge();
            m_nodeA = ContactEdgePool.Instance.Allocate();
            m_nodeA.contact = null;
            m_nodeA.prev = null;
            m_nodeA.next = null;
            m_nodeA.other = null;

            //m_nodeB = new ContactEdge();
            m_nodeB = ContactEdgePool.Instance.Allocate();
            m_nodeB.contact = null;
            m_nodeB.prev = null;
            m_nodeB.next = null;
            m_nodeB.other = null;

            m_toiCount = 0;
        }

        // Update the contact manifold and touching status.
        // Note: do not assume the fixture AABBs are overlapping or are valid.
        public void Update(ContactListener listener)
        {
            Manifold oldManifold = m_manifold;
            
            // Re-enable this contact.
            m_flags |= ContactFlags.e_enabledFlag;

            bool touching = false;
            bool wasTouching = (m_flags & ContactFlags.e_touchingFlag) == ContactFlags.e_touchingFlag;

            bool sensorA = m_fixtureA.IsSensor();
            bool sensorB = m_fixtureB.IsSensor();
            bool sensor = sensorA || sensorB;

            Body bodyA = m_fixtureA.GetBody();
            Body bodyB = m_fixtureB.GetBody();
            Transform xfA = bodyA.Transform;
            Transform xfB = bodyB.Transform;

            // Is this contact a sensor?
            if (sensor)
            {
                Shape shapeA = m_fixtureA.GetShape();
                Shape shapeB = m_fixtureB.GetShape();
                touching = Collision.TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);

                // Sensors don't generate manifolds.
                m_manifold.pointCount = 0;
            }
            else
            {
                Evaluate(out m_manifold, ref xfA, ref xfB);
                touching = m_manifold.pointCount > 0;

                // Match old contact ids to new contact ids and copy the
                // stored impulses to warm start the solver.
                for (int i = 0; i < m_manifold.pointCount; ++i)
                {
                    ManifoldPoint mp2 = m_manifold.points[i];
                    mp2.normalImpulse = 0.0f;
                    mp2.tangentImpulse = 0.0f;
                    ContactID id2 = mp2.id;
                    bool found = false;

                    for (int j = 0; j < oldManifold.pointCount; ++j)
                    {
                        ManifoldPoint mp1 = oldManifold.points[j];

                        if (mp1.id.key == id2.key)
                        {
                            mp2.normalImpulse = mp1.normalImpulse;
                            mp2.tangentImpulse = mp1.tangentImpulse;
                            found = true;
                            break;
                        }

                        oldManifold.points[j] = mp1;
                    }

                    if (found == false)
                    {
                        mp2.normalImpulse = 0.0f;
                        mp2.tangentImpulse = 0.0f;
                    }

                    m_manifold.points[i] = mp2; ////////TODO: check if necessary
                }

                if (touching != wasTouching)
                {
                    bodyA.Awake = true;
                    bodyB.Awake = true;
                }

            }
            
            if (touching)
            {
                m_flags |= ContactFlags.e_touchingFlag;
            }
            else
            {
                m_flags &= ~ContactFlags.e_touchingFlag;
            }
            
            if (wasTouching == false && touching == true && listener != null)
            {
                listener.BeginContact(this);
            }

            if (wasTouching == true && touching == false && listener != null)
            {
                listener.EndContact(this);
            }

            if (sensor == false && touching && listener != null)
            {
                listener.PreSolve(this, oldManifold);
            } 
        }

	    protected static bool s_initialized;

	    internal ContactFlags m_flags;

	    // World pool and list pointers.
	    internal Contact m_prev;
	    internal Contact m_next;

	    // Nodes for connecting bodies.
	    internal ContactEdge m_nodeA;
	    internal ContactEdge m_nodeB;

	    internal Fixture m_fixtureA;
	    internal Fixture m_fixtureB;

        internal int m_indexA;
        internal int m_indexB;

	    internal Manifold m_manifold;

	    internal int m_toiCount;
        internal float m_toi;
    };
}
